Interactive Web Graphics with Shout 3D/Interactive Web Graphics With Shout 3D.iso/mac/Shout3Ddemo/Shout3d_runtime/codebase/applets/WalkPanel.java

Text File | 2000-11-09 | 36KB | 1,018 lines

/**
    Company:        Eyematic Interfaces
    Project:        Shout3D 2.0 Sample Code
    Class:            WalkPanel
    Date:            September 15, 1999
    Description:    Class for Walking
    (C) Copyright Eyematic Interfaces, Inc. - 1997-2000 - All rights reserved
 */

package applets;

import shout3d.core.*;
import shout3d.math.*;
import shout3d.*;

/**
 * Shout3D WalkPanel. This class provides the user with the ability to
 * navigate around a 3D world in a similar manner to the VRML "WALK" mode.
 *
 * Left Mouse Button:
 * -- click and drag to WALK.  Up/down motion moves forward/backward.  Left/right motion
 *    rotates left/right.
 * -- <Shift>+drag to walk at DOUBLE SPEED.
 * -- <Control>+drag to TEMPORARILY LOOK AROUND. Rotates the camera relative to the
 *    forward direction.  Here left/right rotates left/right, and up/down rotates up/down.
 *    As soon as you release the <control> key, the camera snaps back to face forward.
 *
 * Right Mouse Button:
 * -- Works same as <Control>+drag of left mouse button.
 *
 *
 * Features:
 * -- collision detection: Camera will collide with and slide along walls.  True whether
 * or not terrain following is on.
 * -- terrain following and gravity:  By default, camera will follow the terrain and drop
 * via gravity if too high above ground.  Terrain following may be turned off via an
 * applet parameter
 * -- camera space may be rotated: The up direction is always based on the local up
 * direction of the camera, so cameras can walk on walls if the transforms above them
 * turn them sideways.
 * -- robust:  The applet will continue to work properly if the camera or scene is changed
 *
 *
 * Applet Parameters:
 * The following describes applet parameters and their affects:
 *
 * avatarHeight, collideHeight, avatarRadius:
 * These are always expressed as lengths in world space.
 * -- avatarHeight (default 2)
 *    how far the camera is placed above the ground.
 *    When terrainFollowing, this adjusts the camera vertically to hug the ground.
 *    When not terrainFollowing, the height stays constant in the camera's local space.
 * -- collideHeight (default .25)
 *    Collisions with walls are done in the plane that lies at the
 *    level of "collideHeight."  When terrainFollowing, collideHeight implies
 *    the maximum height of what you can step over.
 * -- avatarRadius (default 2)
 *    the closest the camera's collide point may get to a wall.
 *
 * speed controls:
 * These control how fast the camera moves in response to mouse drags.
 * -- forwardDragSpeed (default .05)
 *    Specifies amount of forward velocity per pixel moved. So dragging 100 pixels
 *    moves at 5 units/second.
 * -- rotateDragSpeed (default .0025)
 *    Specifies amount of rotational velocity per pixel moved. So dragging 100 pixels
 *    rotates at .25 radians/second.
 *
 * terrain following controls:
 * -- terrainFollowing (default true)
 *    If true, then the height is kept to be avatarHeight above the ground, where the
 *    ground is considered  the point of intersection that lies below the camera (where
 *    "below" means downward in the camera's local space).  If the camera is navigated
 *    to a point higher than that above the ground, it will drop based on the value of
 *    "gravity."  If terrainFollowing is false, then the camera just remains at a constant
 *    height of avatarHeight in local space and gravity is ignored.
 * -- gravity (default -9.8 units/sec-squared)
 *    If terrainFollowing is true, this controls how quickly the camera will drop when it
 *    is higher than avatarHeight above the ground.  If terrainFollowing is false, this is
 *    ignored.
 * -- maxClimbAngle (default 0.785 radians or about 45 degrees)
 *    If terrainFollowing is true, slopes less than this amount may be climbed (i.e., you
 *    can walk up them) and higher slopes are treated as walls.  If terrainFollowing is
 *    false, this is ignored.
 *
 * @author Paul Isaacs
 * @author Jim Stewartson
 * @author Rory Lane Lutter
 * @author Dave Westwood
 */

public class WalkPanel extends Shout3DPanel implements DeviceObserver{

    ///////////////////////////////////////////////////////////////////
    // Values of parameters that control the applets behavior.
    // These may be changed with applet parameters:
    // See top of file for descriptions of the applet parameters.
    ///////////////////////////////////////////////////////////////////
    // avatarRadius, avatarHeight, and collideHeight
    public float avatarHeight = 2f;
    public float collideHeight = .25f;
    public float avatarRadius = 2f;
    // speed controls.
    public float forwardDragSpeed = .05f;
    public float rotateDragSpeed = .0025f;
    // Terrain following controls.
    public boolean terrainFollowing = true;
    public float   gravity          = -9.8f;
    public float   maxClimbAngle    = 0.785f;

    ///////////////////////////////////////////////////////////////////
    // Public methods to call to control the behavior
    ///////////////////////////////////////////////////////////////////
    /**
     * call this whenever you want to go reset the camera.
     * Result is that camera goes back to its initial orientation and position.
     */
    public void resetCamera(){
        wantToReset = true;
    }

    /**
     * Get the forward drag speed
     */
    public float getForwardDragSpeed(){
        return forwardDragSpeed;
    }
    /**
     * Set the forward drag speed
     */
    public void setForwardDragSpeed(float newSpeed){
        forwardDragSpeed = newSpeed;
    }

    /**
     * Get the heading drag speed
     */
    public float getRotateDragSpeed(){
        return rotateDragSpeed;
    }
    /**
     * Set the heading drag speed
     */
    public void setRotateDragSpeed(float newSpeed){
        rotateDragSpeed = newSpeed;
    }

    ///////////////////////////////////////////////////////////////////
    // Protected member variables used in doing calculations
    ///////////////////////////////////////////////////////////////////

    // Info about the camera, the scene, and the camera's place in the scene.
    Viewpoint camera;
    Transform root;
    Node[]    pathToCameraParent = null;

    // initial camera information and whether there is a request to reset it.
    boolean wantToReset = false;
    boolean gotInitCamera = false;
    float   initCameraHeading;
    float[] initLocalCameraPosition = new float[3];

    // The current state of the camera:
    float   cameraHeading = 0;
    float[] worldCameraPosition = new float[3];
    float   verticalVelocity = 0;    // Only used when terrainFollowing
    float   headingSpeed = 0;
    float   forwardSpeed = 0;
    // For temporarily rotating head relative to the official cameraHeading
    // when the <control> key is pressed and the mouse is then dragged
    float    offsetHeading = 0;
    float    offsetPitch   = 0;
    float    offsetHeadingSpeed = 0;
    float    offsetPitchSpeed   = 0;
    boolean wasControlKeyDown   = false;

    // stored for calculating speeds based on drag distance.
    float startMouseX = 0;
    float startMouseY = 0;

    // For converting points and directions between camera and world spaces.
    // Updated by updateConversionMatrices() & updateCameraQuat()
    float[] cameraToWorldMatrix = new float[16];
    float[] worldToCameraMatrix = new float[16];
    // quaternion for the camera
    Quaternion cameraQuat = new Quaternion();

    // Worldspace representations of camera's directions.
    // Updated by updateUpForwardLeft()
    float[] worldUp         = new float[3];
    float[] worldForward = new float[3];
    float[] worldLeft    = new float[3];
    // Multiply this by a height in worldspace to find same height expressed
    // in camera's local space.  Updated by updateUpForwardLeft()
    float   worldToCameraHeightMultiplier = 1f;

    // These contain info about picks in 3 important directions.
    Picker downwardPicker  = null;    // To find ground below.
    Picker alongGroundPicker = null;  // To find walls ahead/behind.
    Picker alongWallPicker = null;    // To find limits to deflection along wall.
    // These are the points from which the picks are done for the second two
    // pickers.  The downwardPicker just picks down from worldCameraPosition.
    float[] alongWallPickerFrom = { 0, 0, 0 };
    float[] alongGroundPickerFrom = { 0, 0, 0 };

    // Directions of travel used when attempting to move forward/backward
    // or deflect off walls.
    //
    // The direction of travel (forward or backward depending on speed),
    // adjusted by the slope of the ground when terrain following.
    float[] alongGroundDir = { 0, 0, 0 };
    // Set during updateAlongGroundDir.  True if the alongGroundDir is
    // climbing a slope, false in all other cases.
    boolean isClimbing = false;
    // If deflected against a wall, this is the direction of travel parallel to the wall
    float[] alongWallDir = { 1, 0, 0 };

    // Used here and there as temp storage
    float[] tempVec = new float[3];

    ///////////////////////////////////////////////////////////////////
    // Standard panel methods
    ///////////////////////////////////////////////////////////////////
    /**
     * Construct a WalkPanel
     *
     * @param applet the Shout3DApplet in which this panel is to be drawn
     */
    public WalkPanel(Shout3DApplet applet){
        super(applet);
    }
    /**
     * Constructs a WalkPanel
     *
     * @param applet the Shout3DApplet in which this panel is to be drawn
     * @param width the width of the panel in pixels
     * @param height the height of the panel in pixels
     */
    public WalkPanel(Shout3DApplet applet, int width, int height){
        super(applet,width,height);
    }

    /**
     * Constructs a WalkPanel
     *
     * @param applet the Shout3DApplet in which this panel is to be drawn
     * @param x the x position of the panel in pixels
     * @param y the x position of  the panel in pixels
     * @param width the width of the panel in pixels
     * @param height the height of the panel in pixels
     */
    public WalkPanel(Shout3DApplet applet, int x, int y, int width, int height){
        super(applet,x,y,width,height);
    }

    /**
     * Remove observers when done with the panel
     */
    public void finalize()throws Throwable {
        applet.getDeviceListener().removeDeviceObserver(this, "DeviceInput");
        applet.getRenderer().removeRenderObserver(this);
        super.finalize();
    }

    /**
     * Overrides Shout3DPanel.customInitialize()
     *
     * Reads in all the applet parameters.
     * Registers to observe deviceInputs and rendering.
     */
    public void customInitialize() {

        // Read the 3 avatar parameters from applet parameters, if specified:
        String avatarHeightString = applet.getParameter("avatarHeight");
        if (avatarHeightString != null){
            avatarHeight = Float.valueOf(avatarHeightString).floatValue();
        }
        String collideHeightString = applet.getParameter("collideHeight");
        if (collideHeightString != null){
            collideHeight = Float.valueOf(collideHeightString).floatValue();
        }
        String avatarRadiusString = applet.getParameter("avatarRadius");
        if (avatarRadiusString != null){
            avatarRadius = Float.valueOf(avatarRadiusString).floatValue();
        }

        // Read the 2 drag speed parameters, if specified:
        String forwardDragSpeedString = applet.getParameter("forwardDragSpeed");
        if (forwardDragSpeedString != null){
            forwardDragSpeed = Float.valueOf(forwardDragSpeedString).floatValue();
        }
        String rotateDragSpeedString = applet.getParameter("rotateDragSpeed");
        if (rotateDragSpeedString != null){
            rotateDragSpeed = Float.valueOf(rotateDragSpeedString).floatValue();
        }

        // Read the 3 terrain following control parameters, if specified.
        String terrainString = applet.getParameter("terrainFollowing");
        if (terrainString != null && terrainString.toLowerCase().equals("true")){
            terrainFollowing = true;
        }
        String gravityString = applet.getParameter("gravity");
        if (gravityString != null){
            gravity = Float.valueOf(gravityString).floatValue();
        }
        String maxClimbAngleString = applet.getParameter("maxClimbAngle");
        if (maxClimbAngleString != null){
            maxClimbAngle = Float.valueOf(maxClimbAngleString).floatValue();
        }

        // register for device events
        getDeviceListener().addDeviceObserver(this, "DeviceInput", null);
        // register for render events
        getRenderer().addRenderObserver(this, null);

    }

    /**
     * Watch the device inputs:
     * Set the headingSpeed and forwardSpeed based on
     * mouse movement, rotateDragSpeed, and forwardDragSpeed.
     */
    public boolean onDeviceInput(DeviceInput di, Object userData){
        if (di instanceof MouseInput){
            MouseInput mi = (MouseInput)di;
            boolean isControlKeyDown = ((mi.modifiers & DeviceInput.CTRL_MASK) != 0);
            switch (mi.which){
            case MouseInput.DOWN:
                dragStart(mi);
                break;
            case MouseInput.DRAG:
                // Right mouse button is always "look around" but
                // with left button, control key shifts modes.
                // So only restart if using left button and changing
                // control key status
                if (mi.button == 0 && isControlKeyDown != wasControlKeyDown){
                    // Do a restart to change modalities, then continue on
                    dragFinish(mi);
                    dragStart(mi);
                    wasControlKeyDown = isControlKeyDown;
                }
                dragMiddle(mi);
                break;
            case MouseInput.UP:
                dragFinish(mi);
                break;

            }
        }
        // return false, let other entities handle the events too.
        return false;
    }

    protected void dragStart(MouseInput mi) {
        startMouseX = mi.x;
        startMouseY = mi.y;
    }
    protected void dragMiddle(MouseInput mi) {
        // Left mouse without control key down is regular walk mode
        if (mi.button == 0 && !wasControlKeyDown){
            // Regular motion
            if ((mi.modifiers & DeviceInput.SHIFT_MASK) != 0){
                // go fast
                headingSpeed = -(mi.x-startMouseX)*rotateDragSpeed*2;
                forwardSpeed = -(mi.y-startMouseY)*forwardDragSpeed*2;
            }
            else {
                // otherwise go normal speed
                headingSpeed = -(mi.x-startMouseX)*rotateDragSpeed;
                forwardSpeed = -(mi.y-startMouseY)*forwardDragSpeed;
            }
        }
        else {
            // Right mouse or control key down is look-around mode.

            // Offset motion of camera pitch and heading.
            offsetHeadingSpeed = -(mi.x-startMouseX)*rotateDragSpeed;
            offsetPitchSpeed   = -(mi.y-startMouseY)*rotateDragSpeed;
        }
    }
    protected void dragFinish(MouseInput mi) {
        headingSpeed = 0;
        forwardSpeed = 0;
        offsetHeading = 0;
        offsetPitch   = 0;
        offsetHeadingSpeed = 0;
        offsetPitchSpeed   = 0;
    }

    ///////////////////////////////////////////////////////////////////
    // Methods for performing navigation.
    ///////////////////////////////////////////////////////////////////


    /**
     *
     * High-level description:
     *
     * Each time the panel renders, this method will:
     * -- re-initialize the camera if needed
     * -- update the heading orientation
     * -- move forward/backward as far as possible
     * -- deflect off a wall if it couldn't go forward/backward all the way
     * -- adjust the height based on avatarHeight and/or gravity.
     *
     * Note: no onPostRender() method is implemented here because it's
     * implemented in the super class and is not required here.
     *
     */
    public void onPreRender(Renderer r, Object userData){
        super.onPreRender(r, userData);

        // Initialize if necessary.
        checkCamera();

        // adjust the camera heading according to user input
        if (headingSpeed != 0f) {
            cameraHeading += headingSpeed/getFramesPerSecond();
        }
        // adjust the offset heading and pitch according to use input
        if (offsetHeadingSpeed != 0f) {
            offsetHeading += offsetHeadingSpeed/getFramesPerSecond();
        }
        if (offsetPitchSpeed != 0f) {
            offsetPitch += offsetPitchSpeed/getFramesPerSecond();
            // Clamp this to +/- 90 degrees
            if (offsetPitch > 1.57f)
                offsetPitch = 1.57f;
            if (offsetPitch < -1.57f)
                offsetPitch = -1.57f;
        }
        // Set the orientation field based on updated values.
        updateViewpointOrientationField();

        // Insure that conversion matrices and important directions are up to date.
        updateConversionMatrices();
        updateCameraQuat();
        updateUpForwardLeft();

        // How far do we want to go:
        float   distance = (float)(forwardSpeed/getFramesPerSecond());

        // Before trying to move camera, get worldCameraLocation by
        // converting position field to world space.
        // Normally not required, but if the camera lies within a moving coordinate
        // system, this will keep us up to date with where it's been moved.
        convertVecFromTo(cameraToWorldMatrix,camera.position.getValue(),worldCameraPosition);

        // Tries to move along ground.
        // Different behavior depending on value of terrainFollowing.
        boolean isForward = (distance >= 0);
        if (!isForward)
            distance *= -1;
        // This always returns a positive number.
        float distanceMoved = moveCameraAlongGround(isForward, distance);

        if (distanceMoved < distance){
            // Movement was blocked by a wall.
            // Try to deflect and slide parallel to the wall.
            deflectCameraAlongWall(distance - distanceMoved);
        }

        // Adjust camera up/down.  Different behavior depending on value of terrainFollowing
        // and gravity.
        adjustCameraHeight();
        // Set the camera position based on the new worldCameraPosition.
        updateViewpointPositionField();
    }

    /**
     * Check if the camera needs to be re-established.
     * This happens when any of the following occurs:
     * - the scene changes
     * - the bound camera changes.
     * - the path to the camera changes
     *
     * Then check if wantToReset is true and therefore
     * the camera position and rotation need to be reset.
     * This happens either:
     * - the camera was re-established above, or
     * - resetCamera() was called since last time.
     */
    public void checkCamera(){

        // get the current camera and scene root.
        Viewpoint curCam = (Viewpoint)(applet.getCurrentBindableNode("Viewpoint"));
        Transform curRoot = (Transform)getScene();

        // Do they differ from cached values, or is pathToCameraParent no longer valid?
        if (curCam != camera || curRoot != root || !isPathValid(pathToCameraParent)){
            camera = curCam;
            root = curRoot;
            pathToCameraParent = getPathToCameraParent();

            gotInitCamera = false;

            wantToReset = true;
        }

        // Save the initial camera information if needed
        if (gotInitCamera == false && camera != null){
            // Save initial position of camera in local space
            System.arraycopy(camera.position.getValue(), 0, initLocalCameraPosition, 0, 3);

            // Decompose orientation into eulers and save the camera heading.
            // Discard the pitch and roll components.
            Quaternion initQuat = new Quaternion();
            initQuat.setAxisAngle(camera.orientation.getValue());
            float[] initEulers = new float[3];
            initQuat.getEulers(initEulers);
            initCameraHeading = initEulers[0];

            gotInitCamera = true;
        }

        // Do we want to reset the camera position/orientation?
        if (wantToReset == true){
            // Initialize cameraHeading to its proper current value.
            cameraHeading = initCameraHeading;

            // update conversion matrices and important directions
            updateConversionMatrices();
            updateCameraQuat();
            updateUpForwardLeft();

            // Convert initial local camera position to current world position
            convertVecFromTo(cameraToWorldMatrix,
                             initLocalCameraPosition, worldCameraPosition);

            // Adjust the height of the worldspace camera.
            // This brings worldCameraPosition to its proper current value.
            adjustCameraHeight();

            // avoid doing this again
            wantToReset = false;
        }
    }

    /**
     * Insure that the cameraToWorldMatrix and worldToCameraMatrix are up to date.
     */
    void updateConversionMatrices() {
        cameraToWorldMatrix = MatUtil.getMatrixAlongPath(pathToCameraParent, true);
        worldToCameraMatrix = MatUtil.getMatrixAlongPath(pathToCameraParent, false);
    }

    /**
     * Insure that cameraQuat is up to date.
     */
    void updateCameraQuat(){
        cameraQuat.setEulers(cameraHeading, 0, 0);
    }

    final float[] localUp      = { 0, 1, 0 };
    final float[] localForward = { 0, 0, -1 };
    /**
     * Insure that the camera's up, forward and left directions in worldspace are
     * up to date.  Assumes that updateConversionMatrices() has been called.
     */
    void updateUpForwardLeft(){
        // To transform directions from through-the-camera to worldspace,
        // rotate by cameraQuat, then transform by cameraToWorldMatrix,
        // then normalize result.

        System.arraycopy(localForward,0,worldForward,0,3);
        cameraQuat.xform(worldForward);
        convertDirFromTo(cameraToWorldMatrix, worldForward, worldForward );
        MatUtil.normalize(worldForward);

        System.arraycopy(localUp,0,worldUp,0,3);
        cameraQuat.xform(worldUp);
        convertDirFromTo(cameraToWorldMatrix, worldUp, worldUp );
        // Normalize the up vector.
        // The length of the up vector scales heights from camera to world space
        float length = MatUtil.normalize(worldUp);
        // The inverse of the length scales heights from world space to camera space.
        worldToCameraHeightMultiplier = 1/length;

        crossProduct(worldUp, worldForward, worldLeft );
    }


    /**
     * Tries to move along the ground by distance.
     *
     * If no terrainFollowing, will slide along surfaces that are encountered as if they are
     * walls, always staying in same plane of motion.
     *
     * If terrainFollowing, then maxClimbAngle (an applet parameter) defines the maximum slope of
     * ground that can be climbed.  Ground can be climbed.  If a slope
     * is too great to be ground, it is treated as wall and sliding occurs against it.
     *
     * @param isForward true if the camera is trying to move forward relative to its orientation,
     * false if trying to move backward
     * @param desiredDistance how far the camera would like to travel, this is always a positive number
     */
    float moveCameraAlongGround(boolean isForward, float desiredDistance){

        // You might think that a downward pick needs to be done via updateDownwardPick() here.
        // But actually, the information is always there from the render before, because
        // updateDownwardPick() is called during adjustCameraHeight at the end of onPreRender(),
        // as well as during checkCamera() if the camera has changed.
        // So the downwardPicker info is all up to date.

        // Sets alongGroundDir based on terrain (if terrainFollowing) or simply camera's
        // forward (or backward) direction if not terrainFollowing.
        updateAlongGroundDir(isForward);

        // Uses alongGroundPicker to pick in the alongGroundDir direction from the camera.
        updateAlongGroundPick();

        float   actualDistance = 0f;
        if (alongGroundPicker.getPickPath() == null){
            // No obstructions, go entire distance
            actualDistance = desiredDistance;
        }
        else {
            float[] travelHit = alongGroundPicker.getPickInfo(Picker.POINT);

            // Find the distance between the alongGroundPickerFrom point and the pick.
            float distToHit = getDistance(alongGroundPickerFrom, travelHit);

            if (distToHit >= desiredDistance + avatarRadius)
                actualDistance = desiredDistance;
            else if (distToHit <= avatarRadius)
                actualDistance = 0;
            else {
                // Can only get as close to hit as avatarRadius
                actualDistance = distToHit - avatarRadius;
            }
        }

        for (int i = 0; i < 3; i++)
            worldCameraPosition[i] += actualDistance * alongGroundDir[i];

        // Return the distance traveled
        return actualDistance;
    }

    /**
    * Used when movement along ground was blocked by a wall.
    * Tries to slide parallel to the wall that was hit, but in plane of ground.
    *
    * If no terrainFollowing, will just get deflected in ground plane.
    * If terrainFollowing, will follow the direction where the ground plane intersects the
    * wall, which may slope upward in addition to deflecting sideways
    *
    * @return the distance that was actually moved.
    */
    float deflectCameraAlongWall(float desiredDistance) {

        // Finds the direction of travel parallel to the wall.
        updateAlongWallDir();

        // Uses alongWallPicker to pick in the alongWallDir direction from the camera.
        updateAlongWallPick();

        float   actualDistance = 0f;
        if (alongWallPicker.getPickPath() == null){
            // No obstructions, go entire distance
            actualDistance = desiredDistance;
        }
        else {
            float[] travelHit = alongWallPicker.getPickInfo(Picker.POINT);

            // Find the distance between the alongWallPickerFrom point and the pick.
            float distToHit = getDistance(alongWallPickerFrom, travelHit);

            if (distToHit >= desiredDistance + avatarRadius)
                actualDistance = desiredDistance;
            else if (distToHit <= avatarRadius)
                actualDistance = 0;
            else {
                // Can only get as close to hit as avatarRadius
                actualDistance = distToHit - avatarRadius;
            }
        }

        for (int i = 0; i < 3; i++)
            worldCameraPosition[i] += actualDistance * alongWallDir[i];

        // Return the distance traveled
        return actualDistance;
    }

    /**
     * Adjusts the height of the camera over the ground plane.
     *
     */
    void adjustCameraHeight(){
        if (!terrainFollowing){
            // Bring camera position into local space, raise by avatarHeight above
            // groundPlane, convert back out to world space.
            convertVecFromTo(worldToCameraMatrix, worldCameraPosition, tempVec);
            tempVec[1] = avatarHeight * worldToCameraHeightMultiplier;
            convertVecFromTo(cameraToWorldMatrix, tempVec, worldCameraPosition);
        }
        else {
            // Terrain following.
            updateDownwardPick();

            // If there's ground below, drop down to it:
            if (downwardPicker.getPickPath() != null){
                float[] groundPoint = downwardPicker.getPickInfo(Picker.POINT);

                // Do all this stuff with positive signs, it's easier:
                float heightAboveGround = getDistance(worldCameraPosition, groundPoint);
                float fallingDist = Math.abs(verticalVelocity/getFramesPerSecond());
                if ( (fallingDist + avatarHeight) < heightAboveGround ){
                    // Free fall!
                    // Do the fall and accelerate
                    for (int i = 0; i < 3; i++)
                        worldCameraPosition[i] -= fallingDist * worldUp[i];
                    verticalVelocity += gravity/getFramesPerSecond();
                }
                else {
                    // The ground is hit.
                    // Place on ground and set verticalVelocity back to 0.
                    for (int i = 0; i < 3; i++)
                        worldCameraPosition[i] = groundPoint[i] + avatarHeight * worldUp[i];
                    verticalVelocity = 0;
                }
            }
        }
    }

    /**
     * When terrain following, uses the downwardPicker to pick down below
     * the camera.
     */
    void updateDownwardPick(){
        if (!terrainFollowing)
            return;

        if (downwardPicker == null){
            downwardPicker = getNewPicker();
            downwardPicker.setPickInfo(Picker.POINT, true);
            downwardPicker.setPickInfo(Picker.NORMAL, true);
        }

        // Do this every time, since the scene might have changed.
        downwardPicker.setScene(root);

        // Pick from the camera downward.
        for(int i = 0; i < 3; i++)
            tempVec[i] = worldCameraPosition[i] - worldUp[i];
        downwardPicker.pickClosestFromTo(worldCameraPosition, tempVec);
    }

    /**
     * Updates alongGroundDir.
     * Different whether terrain following or not.
     *
     * @param isForward true if camera is moving forward relative to its own POV.
     */
    void updateAlongGroundDir(boolean isForward){

        // Start by assuming that we're not climbing a hill...
        isClimbing = false;

        // ...and that we'll just go in camera's direction, forward or backward:
        System.arraycopy(worldForward, 0, alongGroundDir, 0, 3);
        if (!isForward){
            for (int i = 0; i < 3; i++)
                alongGroundDir[i] *= -1;
        }


        if (!terrainFollowing){
            // When not terrain following, no modification.
            return;
        }

        // Terrain following case.

        // If no ground, no modification, just float the camera's innate direction of travel.
        if (downwardPicker.getPickPath() == null)
            return;

        // If ground is a wall that the camera is moving away from,
        // just let it keep going, don't modify slope of travel.
        // This will be true if there's a component of the ground normal in the
        // direction of travel.
        if (MatUtil.dot(downwardPicker.getPickInfo(Picker.NORMAL), alongGroundDir) > 0)
            return;

        // If ground is an unclimbable wall, use worldForward.
        // We'll either be able to jump the wall or we'll slam into it and
        // slide along it.
        if (!isPlaneClimbable(downwardPicker.getPickInfo(Picker.NORMAL)))
            return;

        // The floor is angled up in front of us at a climbable angle.
        isClimbing = true;

        // Set the direction to be the one within the plane that has a forward sense.
        crossProduct(worldLeft, downwardPicker.getPickInfo(Picker.NORMAL), alongGroundDir);
        MatUtil.normalize(alongGroundDir);
        if (!isForward){
            for (int i = 0; i < 3; i++)
                alongGroundDir[i] *= -1;
        }
        return;
    }

    /**
     * Plane is climbable if the angle between worldUp and its normal
     * is less than maxClimbAngle
     */
    boolean isPlaneClimbable(float[] planeNormal){
        float cosMaxClimbAngle = (float) Math.cos(maxClimbAngle);
        return (MatUtil.dot(planeNormal, worldUp) >= cosMaxClimbAngle);
    }

    /**
     * Uses the alongGroundPicker to pick in alongGroundDir direction from the camera's
     * collide point
     *
     */
    void updateAlongGroundPick(){
        if (alongGroundPicker == null){
            alongGroundPicker = getNewPicker();
            alongGroundPicker.setPickInfo(Picker.POINT, true);
            alongGroundPicker.setPickInfo(Picker.NORMAL, true);
        }

        // Do this every time, since the scene might have changed.
        alongGroundPicker.setScene(root);

        // Move from the collidePoint.
        // The point below the camera at collideHeight is used:
        for (int i = 0; i < 3; i++)
            alongGroundPickerFrom[i] = worldCameraPosition[i] + (-avatarHeight + collideHeight)*worldUp[i];

        // See what lies ahead in the alongGroundDir
        for (int i = 0; i < 3; i++)
            tempVec[i] = alongGroundPickerFrom[i] + alongGroundDir[i];
        alongGroundPicker.pickClosestFromTo(alongGroundPickerFrom, tempVec);
    }

    /**
     * Updates alongWallDir
     * Different whether terrain following or not.
     */
    void updateAlongWallDir(){

        if (!isClimbing) {
            // go parallel to wall and parallel to camera's natural ground plane
            crossProduct(worldUp, alongGroundPicker.getPickInfo(Picker.NORMAL), alongWallDir);
        }
        else {
            // go parallel to wall and parallel to the ground we're climbing:
            crossProduct(downwardPicker.getPickInfo(Picker.NORMAL),
                         alongGroundPicker.getPickInfo(Picker.NORMAL), alongWallDir);
        }
        MatUtil.normalize(alongWallDir);

        // Flip sign if the direction chosen is in opposite sense of our alongGroundDir:
        if (MatUtil.dot(alongWallDir, alongGroundDir) < 0){
            for (int i = 0; i < 3; i++)
                alongWallDir[i] *= -1;
        }
    }

    /**
     * Uses the alongWallPicker to pick in alongWallDir direction from the camera.
     *
     */
    void updateAlongWallPick(){
        if (alongWallPicker == null){
            alongWallPicker = getNewPicker();
            alongWallPicker.setPickInfo(Picker.POINT, true);
            alongWallPicker.setPickInfo(Picker.NORMAL, true);
        }

        // Do this every time, since the scene might have changed.
        alongWallPicker.setScene(root);

        // Move from the collide point.
        // The point below the camera at collideHeight is used:
        for (int i = 0; i < 3; i++)
            alongWallPickerFrom[i] = worldCameraPosition[i] + (-avatarHeight + collideHeight)*worldUp[i];

        // See what lies ahead in the alongWallDir
        for (int i = 0; i < 3; i++)
            tempVec[i] = alongWallPickerFrom[i] + alongWallDir[i];
        alongWallPicker.pickClosestFromTo(alongWallPickerFrom, tempVec);
    }

    ///////////////////////////////////////////////////////////////////
    // Methods to set the fields based on current state.
    ///////////////////////////////////////////////////////////////////

    /**
     * Transforms the worldCameraPosition to local space,then
     * sets the field value.
     */
    void updateViewpointPositionField(){
        convertVecFromTo(worldToCameraMatrix,worldCameraPosition,camera.position.getValue());
        camera.position.setValue( camera.position.getValue() );
    }

    /**
     * Viewpoint's orientation is based solely on the heading.
     */
    void updateViewpointOrientationField(){
        // Set the camera orientation based on the cameraHeading
        cameraQuat.setEulers(cameraHeading+offsetHeading, offsetPitch, 0);
        cameraQuat.getAxisAngle(camera.orientation.getValue());
        camera.orientation.setValue(camera.orientation.getValue());
    }

    ///////////////////////////////////////////////////////////////////
    // Simple utility methods for paths, matrices, and vectors.
    ///////////////////////////////////////////////////////////////////

    // Distance function
    static float getDistance(float[] from, float[] to){
        float x = from[0] - to[0];
        float y = from[1] - to[1];
        float z = from[2] - to[2];
        float dist = (float)Math.sqrt(x*x + y*y + z*z);
        //System.out.println(dist);
        return dist;
    }

    static void crossProduct(float[] vec0, float[] vec1, float[] result){
        result[0] =  vec0[1]*vec1[2] - vec0[2]*vec1[1];
        result[1] = -vec0[0]*vec1[2] + vec0[2]*vec1[0];
        result[2] =  vec0[0]*vec1[1] - vec0[1]*vec1[0];
    }

    /**
     * Converts fromVec by the matrix matrix, placing the converted result into
     * toVec.
     *
     * fromVec and toVec may refer to the same vector.
     * matrix, fromVec and toVec must be pre-allocated.
     */
    static void convertVecFromTo(float[] matrix, float[] fromVec, float[] toVec){
        System.arraycopy(fromVec,0,toVec,0,3);
        MatUtil.multVecMatrix(matrix,toVec);
    }

    /**
     * Converts fromDir by the matrix matrix, placing the converted result into
     * toDir.
     *
     * fromDir and toDir may refer to the same vector.
     * matrix, fromDir and toDir must be pre-allocated.
     */
    static void convertDirFromTo(float[] matrix, float[] fromDir, float[] toDir){
        System.arraycopy(fromDir,0,toDir,0,3);
        MatUtil.multDirMatrix(matrix,toDir);
    }

    /**
     * Returns a path from root to camera's parent, if camera lies under scene.
     *
     * If no camera is included in the s3d scene, the panel will use a
     * default camera that is not transformed relative to world space.
     * So in this case, null is returned.
     */
    public Node[] getPathToCameraParent(){
        // Set answer to be a path from root down to camera
        Searcher s = getNewSearcher();
        s.setNode(camera);
        Node[] searchPath = s.searchFirst(root);

        if (searchPath==null || searchPath.length < 2)
            return null;

        // Return a copy of the path with its last node (the viewpoint) removed.
        Node[] answer = new Node[searchPath.length-1];
        System.arraycopy(searchPath,0,answer,0,answer.length);
        return answer;
    }

    /**
     * Returns true if path p is a valid path.
     * A valid path is:
     * --  null
     * --  one node long
     * --  or one in which the parent/child relationships are all correct.
     */
    static boolean isPathValid(Node[] p){

        // null paths are valid
        if (p == null || p.length < 2)
            return true;

        // make sure parent/child relationships still hold.  If not, then
        // reparenting has occured between some of the nodes.
        Node   parent, child;
        Node[] kids;
        // For each parent along the path
        for (int i = 0; i < p.length-2; i++){
            parent = p[i];
            child = p[i+1];
            // Return false if the child is not in the parent's children field.
            if ( !(parent instanceof Group) )
                return false;
            kids = ((Group)parent).children.getValue();
            if (kids == null)
                return false;
            boolean gotIt = false;
            for (int j = 0; j < kids.length; j++){
                if (kids[j] == child)
                    gotIt = true;
            }
            // Return if the child was not in the parent's field
            if (gotIt == false)
                return false;
        }
        // Passed all tests, return true.
        return true;
    }
}